/*
 Startup counter example with Preferences library.

 This simple example demonstrates using the Preferences library to store
 how many times the device has booted.
*/

#include <LD2451.h>

// 初始化雷达对象，使用Serial2
LD2451 radar(Serial2);

// 配置参数
LD2451::Config radarConfig;
LD2451::Sensitivity sensitivity;

void setup() {
    Serial.begin(115200);
    Serial2.begin(115200, SERIAL_8N1, 16, 17); // RX=16, TX=17

    // 等待串口初始化
    delay(1000);

    Serial.println(F("LD2451 Radar Example"));
    Serial.println(F("===================="));

    // 1. 使能配置模式
    Serial.println(F("Enabling config mode..."));
    if (radar.enableConfigMode()) {
        Serial.println(F("Config mode enabled"));
        
        // 2. 读取固件版本
        String fwVersion;
        if (radar.readFirmwareVersion(fwVersion)) {
            Serial.print(F("Firmware version: "));
            Serial.println(fwVersion);
        } else {
            Serial.println(F("Failed to read firmware version!"));
        }
        
        // 3. 设置检测参数
        radarConfig.minDistance = 100;      // 100米
        radarConfig.detectionDirection = 2; // 检测所有方向
        radarConfig.minSpeed = 5;           // 5km/h
        radarConfig.noTargetDelay = 2;      // 2秒
        
        Serial.println(F("Setting detection parameters..."));
        if (radar.setDetectionParams(radarConfig)) {
            Serial.println(F("Detection parameters set successfully"));
        } else {
            Serial.println(F("Failed to set detection parameters!"));
        }
        
        // 4. 读取检测参数验证
        LD2451::Config readConfig;
        if (radar.readDetectionParams(readConfig)) {
            Serial.println(F("Current detection parameters:"));
            Serial.print(F("  Min distance: ")); Serial.print(readConfig.minDistance); Serial.println(F("m"));
            Serial.print(F("  Detection direction: ")); 
            Serial.println(readConfig.detectionDirection == 0 ? "Far" : 
                          readConfig.detectionDirection == 1 ? "Approaching" : "All");
            Serial.print(F("  Min speed: ")); Serial.print(readConfig.minSpeed); Serial.println(F("km/h"));
            Serial.print(F("  No target delay: ")); Serial.print(readConfig.noTargetDelay); Serial.println(F("s"));
        } else {
            Serial.println(F("Failed to read detection parameters!"));
        }
        
        // 5. 设置灵敏度
        sensitivity.triggerCount = 2;
        sensitivity.snrThreshold = 5;
        
        Serial.println(F("Setting sensitivity..."));
        if (radar.setSensitivityParams(sensitivity)) {
            Serial.println(F("Sensitivity set successfully"));
        } else {
            Serial.println(F("Failed to set sensitivity!"));
        }
        
        // 6. 结束配置模式
        Serial.println(F("Ending config mode..."));
        if (radar.endConfigMode()) {
            Serial.println(F("Config mode ended"));
        } else {
            Serial.println(F("Failed to end config mode!"));
        }
    } else {
        Serial.println(F("Failed to enable config mode!"));
    }
    
    Serial.println(F("\nStarting target detection..."));
    Serial.println(F("----------------------------"));
}

void loop() {
    // 处理串口数据
    radar.processSerial();
    
    // 获取并打印目标信息
    const auto& targets = radar.getTargets();
    if (!targets.empty()) {
        Serial.print(F("Detected ")); 
        Serial.print(targets.size());
        Serial.println(F(" targets:"));
        
        for (int i = 0; i < targets.size(); i++) {
            const auto& t = targets[i];
            Serial.print(F("  Target ")); Serial.print(i + 1); Serial.print(": ");
            Serial.print(F("Dist: ")); Serial.print(t.distance); Serial.print(F("m, "));
            Serial.print(F("Speed: ")); Serial.print(t.speed); Serial.print(F("km/h, "));
            Serial.print(F("Angle: ")); Serial.print(t.angle); Serial.print(F("°, "));
            Serial.print(F("Dir: ")); Serial.print(t.isApproaching ? F("Approaching") : F("Moving away"));
            Serial.print(F(", SNR: ")); Serial.println(t.snr);
        }
        Serial.println(F("------------------"));
    }
    
    delay(100); // 适当延迟
}